package osl.ev3;



import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.Motor;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.SampleProvider;
import lejos.robotics.filter.AbstractFilter;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;

public class LineFollower {
	
	static RegulatedMotor leftMotor = Motor.B;
	static RegulatedMotor rightMotor = Motor.C;
	static ColorSensor colorSensor;
	
	public static void main(String[] args){	
		leftMotor.resetTachoCount();
	    rightMotor.resetTachoCount();
	    leftMotor.rotateTo(0);
	    rightMotor.rotateTo(0);
	    leftMotor.setSpeed(100);
	    rightMotor.setSpeed(100);
	    leftMotor.setAcceleration(800);
	    rightMotor.setAcceleration(800);
	    colorSensor = new ColorSensor();
	    colorSensor.setDaemon(true);
	    colorSensor.start();
	    Behavior b1 = new OnTheLine();
	    Behavior b2 = new OffTheLine();
	    Behavior[] behaviorList =
	    {
	      b1, b2
	    };
	    Arbitrator arbitrator = new Arbitrator(behaviorList);
	    LCD.drawString("Line follower",0,1);
	    leftMotor.forward();
	    arbitrator.start();
	}

}

class ColorSensor extends Thread {
	EV3ColorSensor colorSensor = new EV3ColorSensor(SensorPort.S3);
	
	public float value = 0;
	
	ColorSensor(){}
	
	public void run() {
		SampleProvider redMode = colorSensor.getRedMode();
		SampleProvider reflectedLight = new autoAdjustFilter(redMode);
		int sampleSize = reflectedLight.sampleSize();
	    float[] sample = new float[sampleSize];
		while(true){
			reflectedLight.fetchSample(sample, 0);
			value = sample[0];
            System.out.println("Value: " + value);
		}		
	}
}

 class autoAdjustFilter extends AbstractFilter {
    
    private float[] minimum;
    private float[] maximum;

    public autoAdjustFilter(SampleProvider source) {
      super(source);
      
      minimum = new float[sampleSize];
      maximum = new float[sampleSize];
      reset();
    }

    public void reset() {

      for (int i = 0; i < sampleSize; i++) {
        minimum[i] = Float.POSITIVE_INFINITY;
        maximum[i] = Float.NEGATIVE_INFINITY;
      }
    }
    
    public void fetchSample(float[] sample, int offset) {
        super.fetchSample(sample, offset);
        for (int i = 0; i < sampleSize; i++) {
          if (minimum[i] > sample[offset + i])
            minimum[i] = sample[offset + i];
          if (maximum[i] < sample[offset + i])
            maximum[i] = sample[offset + i];
          System.out.println("Original Value: "+ i + sample[offset + i]);
          if(maximum[i] != minimum[i]){
        	  sample[offset + i] = (sample[offset + i] - minimum[i]) / (maximum[i] - minimum[i]);
          }
        }
      }
 }
 
 class OnTheLine implements Behavior
 {

	@Override
	public boolean takeControl() {
		// TODO Auto-generated method stub
		return true;
	}

	@Override
	public void action() {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void suppress() {
		// TODO Auto-generated method stub
		
	}
	 
 }
 
 class OffTheLine implements Behavior
 {
	 
	 private static boolean leftMotorMoving = false;

	 private boolean checkLine()
	  {

	      float value = LineFollower.colorSensor.value;
	      if (value > 0.30)
	      {
	    	  leftMotorMoving = LineFollower.leftMotor.isMoving();
	    	  /*LineFollower.leftMotor.stop();
	    	  LineFollower.rightMotor.stop();*/
	    	  System.out.println("Vysiel som z ciary");
	          return true;
	      }
	      else
	      {
	          return false;
	      }
	  }
	 
	@Override
	public boolean takeControl() {
		// TODO Auto-generated method stub
		return checkLine();
	}

	@Override
	public void action() {
		// TODO Auto-generated method stub
		if(leftMotorMoving){
			System.out.println("Vypinam lavy motor a zapinam pravy");
			LineFollower.leftMotor.stop();
			LineFollower.rightMotor.forward();
			while(LineFollower.colorSensor.value > 0.30){
				System.out.println("Tocim sa spat na ciaru");
			}
			System.out.println("Mal by som byt naspat na ciare");
		} else {
			System.out.println("Vypinam pravy motor a zapinam lavy");						
			LineFollower.rightMotor.stop();
			LineFollower.leftMotor.forward();
			while(LineFollower.colorSensor.value > 0.30){
				System.out.println("Tocim sa spat na ciaru");
			}
			System.out.println("Mal by som byt naspat na ciare");
		}		
	}

	@Override
	public void suppress() {
		
	}
	 
 }
